equations of motion with matlab